/*
 * Copyright (c) 2022, www.lgmgim.cn
 * All rights reserved.
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *	* Redistributions of source code must retain the above copyright
 * 	  notice, this list of conditions and the following disclaimer.
 * 	* Redistributions in binary form must reproduce the above copyright
 *	  notice, this list of conditions and the following disclaimer in the
 * 	  documentation and/or other materials provided with the distribution.
 * 	* Neither the name of the University of California, Berkeley nor the
 * 	  names of its contributors may be used to endorse or promote products
 * 	  derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

/*
 * @file peripheral.cpp
 * @author haibo
 * @date Sep 1, 2022
 * @brief Example code to control peripheral (jecking, rotary, forklift, roller, etc)
 * Contact: haibo.yang@lgmgim.cn
 */

#include <agv_peripheral_controller/multiJack/peripheral_two_jacking.h>

using namespace std;
#define PICK_NAME_CONFIG "/agv_peripheral_controller/pick_name"
#define DROP_NAME_CONFIG "/agv_peripheral_controller/drop_name"
#define ADJUST_NAME_CONFIG "/agv_peripheral_controller/adjust_name"

#define ORDER_UP_OLD_VERSION "1"	  // 兼容旧版本动作指令动作上升指令
#define ORDER_DOWN_OLD_VERSION "2" // 兼容旧版本动作下降指令
namespace agv_peripheral
{

	AgvPeripheral::AgvPeripheral(const std::string &name)
		: name(name), frequency(50.0f), shutdown_req(false),
		  as(nh, "do_peripheral", false)
	// ,dynamic_reconfigure_server(NULL)
	{
		memset(&os, 0, sizeof(os));

		as.registerGoalCallback(boost::bind(&AgvPeripheral::goalCB, this));
		as.registerPreemptCallback(boost::bind(&AgvPeripheral::preemptCB, this));
		as.start();

		find_joint_id = false;
		joint_id[0] = 0;
		joint_id[1] = 1;

		manual_jacking_active = false;

		read_record_result = false;
		last_event_time = rclcpp::Time::now();
		last_set_stop_time = rclcpp::Time::now();
		last_landmark_time = rclcpp::Time::now();

		read_input_data.reset = false;
		read_input_data.emergency_stop_1 = false;
		read_input_data.emergency_stop_2 = false;

		input_status.upControl = false;
		input_status.downControl = false;

		last_read_input_data.reset = false;
		last_read_input_data.emergency_stop_1 = false;
		last_read_input_data.emergency_stop_2 = false;

	}

	AgvPeripheral::~AgvPeripheral()
	{
		m_quit_mutex.unlock();
		m_thread.join();
	}

	void AgvPeripheral::clear_os()
	{
		std::mutex::scoped_lock lock(m_mutex);
		memset(&os, 0, sizeof(os));
		RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] clear os value");
	}

	void AgvPeripheral::set_cur_op(periph_op_enum op)
	{
		std::mutex::scoped_lock lock(m_mutex);
		if (op)
		{
			operate_time = rclcpp::Time::now();
		}
		os.cur_op = op;
		RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] set op %d", os.cur_op);
	}

	int AgvPeripheral::get_cur_op()
	{
		std::mutex::scoped_lock lock(m_mutex);
		int op = os.cur_op;
		return op;
	}

	void AgvPeripheral::set_next_op(periph_op_enum op)
	{
		std::mutex::scoped_lock lock(m_mutex);
		os.next_op = op;
	}

	int AgvPeripheral::get_next_op()
	{
		std::mutex::scoped_lock lock(m_mutex);
		int op = os.next_op;
		return op;
	}

	void AgvPeripheral::set_cur_state(periph_stata_enum state)
	{
		std::mutex::scoped_lock lock(m_mutex);
		if (os.cur_state != state)
		{
			os.cur_state = state;
			RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] set cur_state %d", os.cur_state);
		}
	}

	int AgvPeripheral::get_cur_state()
	{
		std::mutex::scoped_lock lock(m_mutex);
		int state = os.cur_state;
		return state;
	}

	/** @brief Callback function for the Joint State subscriber */
	void AgvPeripheral::jointStateCallback(const sensor_msgs::msg::JointState::ConstSharedPtr &msg)
	{
		jointState_update_time = rclcpp::Time::now();
		joint_state = *msg;
		if (!find_joint_id)
		{
			bool has_jacking = (peripheral_type == JACKING) || (peripheral_type == JACKING_ROTARY);
			bool find_jacking[2] = {false, false};

			for (int i = 0; i < joint_state.name.size(); i++) // find jacking and rotary joint
			{
				if (has_jacking && !find_jacking[0] && joint_state.name[i].find(JACKING_JOINT_1) != string::npos)
				{
					joint_id[0] = i;
					find_jacking[0] = true;
					RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] JACKING ID:%d", joint_id[0]);
				}

				if (has_jacking && !find_jacking[1] && joint_state.name[i].find(JACKING_JOINT_2) != string::npos)
				{
					joint_id[1] = i;
					find_jacking[1] = true;
					RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] ROTARY ID:%d", joint_id[1]);
				}
			}

			if (has_jacking && (!find_jacking[0] || !find_jacking[1]))
			{
				RCLCPP_ERROR(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] Could not find peripheral jonit number");
			}
			find_joint_id = true;
		}

		if (find_joint_id)
		{
			joint_pos[0] = joint_state.position[joint_id[0]];
			joint_pos[1] = joint_state.position[joint_id[1]];
			periphMotor[0].pos = joint_pos[0];
			periphMotor[1].pos = joint_pos[1];
		}
	}

	void AgvPeripheral::check_jointState_out()
	{
		if ((((rclcpp::Time::now() - jointState_update_time).toSec() * 1000) > 1000) && jointState_update_time.toSec()) // 1s
		{
			RCLCPP_ERROR(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] check joint state timeout");
			set_cur_state(UNCONTROLLED_STATE);
		}
	}

	void AgvPeripheral::agvModecb(const agv_msgs::msg::AGVMode::ConstSharedPtr &msg)
	{
		uint8_t agv_mode = msg->mode;
		input_status.manualState = agv_mode == agv_msgs::msg::AGVMode::MANAUL;
	}

	void AgvPeripheral::landmarkShelfCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr &data)
	{
		last_landmark_shelf_time = rclcpp::Time::now();
		loadInfo.loadId = to_string((int)trunc(data->pose.position.z));
	}

	void AgvPeripheral::landmarkCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr &data)
	{
		last_landmark_time = rclcpp::Time::now();
	}

	void AgvPeripheral::driversInfoCallback(const agv_msgs::msg::DriversInfo::ConstSharedPtr &data)
	{
		driversInfo = *data;
		bool has_jacking = (peripheral_type == JACKING) || (peripheral_type == JACKING_ROTARY);

		uint16_t statusWord = 0x0;

		for (int i = 0; i < driversInfo.drivers.size(); i++)
		{
			statusWord = driversInfo.drivers[i].statusWord;

			if (has_jacking && driversInfo.drivers[i].jointName.find(JACKING_JOINT_1) != string::npos)
			{
				motorStatus[0].enableState = ((statusWord & 0x07) == 0x07);
				motorStatus[0].homeState = (statusWord & 0x8000);
				motorStatus[0].motorErr = (statusWord & 0x8);
				if (motorStatus[0].motorErr == 0)
				{
					jacking_recover_emit[0] = false;
				}
				else if (motorStatus[0].motorErr != last_jacking_err[0] && !last_jacking_err[0])
				{
					jacking_recover_emit[0] = true;
					err_begin_time = rclcpp::Time::now();
					RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] recover emit. jacking 1 errcode[%d]", motorStatus[0].motorErr);
				}
				motorStatus[0].movingState = (statusWord & 0x400);
				motorStatus[0].ready = driversInfo.drivers[i].ready;
				periphMotor[0].enableStatus = motorStatus[0].enableState;
				periphMotor[0].homeFinished = motorStatus[0].homeState;
				periphMotor[0].errStatus = motorStatus[0].motorErr;
				periphMotor[0].ready = motorStatus[0].ready;
				if (statusWord != last_jacking_status_word[0])
				{
					RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] jacking 1 motor statusWord: %x", statusWord);
					RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] jacking 1 statusWord[%x] enable[%d] home[%d]", statusWord, motorStatus[0].enableState, motorStatus[0].homeState);
				}
				last_jacking_status_word[0] = statusWord;
				last_jacking_err[0] = (motorStatus[0].motorErr == 0x0);
			}

			if (has_jacking && driversInfo.drivers[i].jointName.find(JACKING_JOINT_2) != string::npos)
			{
				motorStatus[1].enableState = ((statusWord & 0x07) == 0x07);
				motorStatus[1].homeState = (statusWord & 0x8000);
				motorStatus[1].motorErr = (statusWord & 0x8);
				if (motorStatus[1].motorErr == 0)
				{
					jacking_recover_emit[1] = false;
				}
				else if (motorStatus[1].motorErr != last_jacking_err[1] && !last_jacking_err[1])
				{
					jacking_recover_emit[1] = true;
					err_begin_time = rclcpp::Time::now();
					RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] recover emit. jacking 2 errcode[%d]", motorStatus[0].motorErr);
				}
				motorStatus[1].movingState = (statusWord & 0x400);
				motorStatus[1].ready = driversInfo.drivers[i].ready;
				periphMotor[1].enableStatus = motorStatus[0].enableState;
				periphMotor[1].homeFinished = motorStatus[0].homeState;
				periphMotor[1].errStatus = motorStatus[0].motorErr;
				periphMotor[1].ready = motorStatus[0].ready;
				if (statusWord != last_jacking_status_word[0])
				{
					RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] jacking 2 motor statusWord: %x", statusWord);
					RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] jacking 2 statusWord[%x] enable[%d] home[%d]", statusWord, motorStatus[0].enableState, motorStatus[0].homeState);
				}
				last_jacking_status_word[1] = statusWord;
				last_jacking_err[1] = (motorStatus[1].motorErr == 0x0);
			}
		}
	}

	void AgvPeripheral::autoStatusCallback(const agv_msgs::msg::AgvAutoStatus::ConstSharedPtr &msg)
	{
		// if (input_status.manualState)
		// {
		// 	pallet_controller->set_followup(0);
		// }
		// else
		// {
		// 	pallet_controller->set_followup(msg->rackFollowUp);

		// 	uint8_t enablePalletRotate = (msg->bits >> bit_pos_rotate) & 0x1;
		// 	double target = (double)msg->word1 / 180.0 * M_PI * -1;
		// 	if (enablePalletRotate && !msg->rackFollowUp && periphStatus.loaded)
		// 	{
		// 		pallet_controller->set_rotary_target_position(target);
		// 	}
		// }
	}

	void AgvPeripheral::navTypeCallback(const agv_msgs::msg::NavType::ConstSharedPtr &msg)
	{
		nav_type = msg->type;
	}

	void AgvPeripheral::readInputCallback(const agv_msgs::msg::ReadInPutsNew::ConstSharedPtr &msg)
	{
		std::vector<agv_msgs::msg::PairTypeInt> input_msg = msg->array;

		for (const auto &data : input_msg)
		{
			if (data.key.compare("resetButton") == 0)
			{
				read_input_data.reset = (data.value == 1);
			}
			else if (data.key.compare("emergencyButton") == 0)
			{
				read_input_data.emergency_stop_1 = (data.value == 1);
			}
			else if (data.key.compare("emergencyButton2") == 0)
			{
				read_input_data.emergency_stop_2 = (data.value == 1);
			}

			if (data.key == "upControl")
            {
                // 以上升沿为判断
				if (data.value == 1 && input_status.upControl == 0)
				{
					setTargetJackingPosition(max_pos[0]);
					last_jacking_time = rclcpp::Time::now();
				}
				else if (data.value == 0 && input_status.upControl == 1) {
					setTargetJackingPosition(joint_pos[0]);
				}
				input_status.upControl = data.value;
            }

			if (data.key == "downControl")
			{
				if (data.value == 1 && input_status.downControl == 0)
				{
					setTargetJackingPosition(min_pos[0]);
					last_jacking_time = rclcpp::Time::now();
				}
				else if (data.value == 0 && input_status.downControl == 1)
				{
					setTargetJackingPosition(joint_pos[0]);
				}
				input_status.downControl = data.value;
			}

		}
	}

	void AgvPeripheral::periphTeleopCtlcb(const agv_msgs::msg::PeriphTeleop::ConstSharedPtr &msg)
	{
		rclcpp::Time now = rclcpp::Time::now();
		uint8_t order = msg->order;
		float freq = msg->freq;
		double speed_limit = 0.0;
		// ROS_INFO_THROTTLE(1, "[AgvPeripheral] teleop called. order[%d] freq[%.4f]  current_state[%d]", order, freq, get_cur_state());

		if ((get_cur_state() != MANUAL_MODE_STATE) || (!freq))
		{
			return;
		}

		if (order == UP)
		{
			manual_jacking_active = true;
			jacking_timeout_time = 5 * (1 / freq);
			manual_target_pos[0] = max_pos[0];
			setTargetJackingPosition(manual_target_pos[0]);
			last_jacking_time = now;
		}
		else if (order == DOWN)
		{
			manual_jacking_active = true;
			jacking_timeout_time = 5 * (1 / freq);
			manual_target_pos[0] = min_pos[0];
			setTargetJackingPosition(manual_target_pos[0]);
			last_jacking_time = now;
		}
	}

	void AgvPeripheral::set_stop_bits()
	{
		// int pallet_state = pallet_controller->get_state();
		// if (pallet_state == PalletController::TYPE::TASK || pallet_state == PalletController::TYPE::RESET ||
		// 	pallet_state == PalletController::TYPE::FIX)
		// {
		// 	setStop.stop = 1;
		// 	if ((rclcpp::Time::now() - last_set_stop_time).toSec() > 0.02)
		// 	{
		// 		set_stop_pub.publish(setStop);
		// 		last_set_stop_time = rclcpp::Time::now();
		// 	}
		// }
		// else
		// {
		// 	setStop.stop = 0;
		// 	if ((rclcpp::Time::now() - last_set_stop_time).toSec() > 0.1)
		// 	{
		// 		set_stop_pub.publish(setStop);
		// 		last_set_stop_time = rclcpp::Time::now();
		// 	}
		// }
	}


	bool AgvPeripheral::init()
	{
		int type;
		nh.param("/agv_peripheral_controller/PeripheralType", type, 2);
		nh.param("/agv_peripheral_controller/LoadWithQRCode", load_with_qrcode, false);
		nh.param("/agv_peripheral_controller/BitPosRotate", bit_pos_rotate, 8);
		nh.param("/joint/jacking_joint_1/homeNeed", jacking_home_need[0], false);
		nh.param("/joint/jacking_joint_2/homeNeed", jacking_home_need[1], false);

		periphMotor.resize(2);
		if (type == JACKING)
		{
			RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "JACKING");
			peripheral_type = JACKING;
			nh.param<float>("/agv_peripheral_controller/JackingMaxPos", max_pos_user[0], 0.05);
			nh.param<float>("/agv_peripheral_controller/JackingMinPos", min_pos_user[0], 0.00);
			RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "JackingMaxPos: %.4f  JackingMinPos: %.4f", max_pos_user[0], min_pos_user[0]);
			max_pos[0] = max_pos_user[0];
			min_pos[0] = min_pos_user[0];
			periphMotor[0].motor_name = JACKING_JOINT_1;
			periphMotor[1].motor_name = JACKING_JOINT_2;
		}

		// subscribe joint state
		js_sub = nh.subscribe("/base/joint_states", 1, &AgvPeripheral::jointStateCallback, this);
		periph_status_pub = nh.advertise<agv_msgs::msg::PeriphStatus>("/periphStatus", 1);
		periph_teleop_sub = nh.subscribe("/base/periphTeleopInfo", 1, &AgvPeripheral::periphTeleopCtlcb, this);
		// agvInfo_sub = nh.subscribe("agvinfo", 1, &AgvPeripheral::agvInfocb, this);
		agvMode_sub = nh.subscribe("agvmode", 1, &AgvPeripheral::agvModecb, this);

		PeripheralHaltCtl = nh.serviceClient<std_srvs::srv::Trigger>("/base/driver/halt");
		PeripheralRecoverCtl = nh.serviceClient<std_srvs::srv::Trigger>("/base/driver/recover");
		// dynamic_reconfigure_callback = boost::bind(&AgvPeripheral::reconfigCB, this, _1, _2);
		// dynamic_reconfigure_server.setCallback(dynamic_reconfigure_callback);
		event_pub = nh.advertise<agv_msgs::msg::AGVEvent>("/agvevent", 10);
		landmark_shelf_sub = nh.subscribe("/landmark_shelf", 1, &AgvPeripheral::landmarkShelfCallback, this);
		landmark_sub = nh.subscribe("/landmark", 1, &AgvPeripheral::landmarkCallback, this);
		driver_info_sub = nh.subscribe("/base/driversInfo", 1, &AgvPeripheral::driversInfoCallback, this);
		set_stop_pub = nh.advertise<agv_msgs::msg::SetStopBits>("/set_stop_bits", 10);
		auto_status_sub = nh.subscribe("/auto_run_status", 10, &AgvPeripheral::autoStatusCallback, this);
		nav_type_sub = nh.subscribe("/navigation_type", 10, &AgvPeripheral::navTypeCallback, this);
		readinput_sub = nh.subscribe("/readInputNew", 10, &AgvPeripheral::readInputCallback, this);
		outputClient = nh.serviceClient<agv_srvs::srv::OutputsNew>("/outputNew");
	    
	    jacking_1_pub = nh.advertise<std_msgs::msg::Float64>("/base/controllers/jacking_controller_1/command", 10);
	    jacking_2_pub = nh.advertise<std_msgs::msg::Float64>("/base/controllers/jacking_controller_2/command", 10);

		// pos_record_file = ros::package::getPath(PACKAGE_NAME) + FILE_NAME;
		pos_record_file = std::string(getenv("HOME")) + FILE_NAME;
		RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "pos record file path is: %s", pos_record_file.c_str());

		// 确认是否有举升电源继电器
		vector<string> param_names;
		if (ros::param::getParamNames(param_names)) {
			for (auto param : param_names)
			{
				std::size_t pos_out = param.find("/IO/output_device/");
				if(pos_out != std::string::npos) {
					std::string key = param.erase(pos_out, std::string("/IO/output_device/").length());
					std::size_t zone_out = key.find("liftRelay");
					if (zone_out != std::string::npos)
					{
						// 打开举升继电器
						rclcpp::Duration duration(1.0); 
						duration.sleep();
						agv_srvs::srv::OutputsNew lift_relay;
						agv_msgs::msg::PairTypeInt singleIo;
						singleIo.key = "liftRelay";
						singleIo.value = 1;
						lift_relay.request.array.push_back(singleIo);
						if (outputClient.call(lift_relay) && lift_relay.response.state)
						{
							ROS_INFO_STREAM("lift relay is open");
						} else ROS_ERROR_STREAM("lift relay open filed");
					}
				}
			}
		}

		ep_.init(nh);

		read_record_result = periphRecordPosRead();
		m_quit_mutex.lock();
		m_thread = std::thread(thread_fun, this);
		return true;
	}

	bool AgvPeripheral::periphReadyCheck()
	{
		bool periphReady = false;
		if (peripheral_type == JACKING)
		{
			periphReady = motorStatus[0].ready && motorStatus[1].ready;
		}

		if (!periphReady)
			ROS_INFO_THROTTLE(1, "peripheral motor ready check: jacking1[%d] jacking2[%d]", motorStatus[0].ready, motorStatus[1].ready);

		return periphReady;
	}

	bool AgvPeripheral::periphMotorEnableCheck()
	{
		bool periphEnabled = false;
		if (peripheral_type == JACKING)
		{
			periphEnabled = motorStatus[0].enableState && motorStatus[1].enableState;
		}

		if (!periphEnabled)
			ROS_INFO_THROTTLE(1, "peripheral motor enable check: jacking1[%d] jacking2[%d]", motorStatus[0].enableState, motorStatus[1].enableState);

		return periphEnabled;
	}

	bool AgvPeripheral::periphMotorErrCheck()
	{
		bool periphinErr = false;
		if (peripheral_type == JACKING)
		{
			periphinErr = motorStatus[0].motorErr || motorStatus[1].motorErr;
		}

		if (periphinErr)
			ROS_INFO_THROTTLE(1, "peripheral motor error check: jacking1[%d] jacking2[%d]", motorStatus[0].motorErr, motorStatus[1].motorErr);
		return periphinErr;
	}

	bool AgvPeripheral::periphHomingCheck()
	{
		bool periphHomed = false;
		if (peripheral_type == JACKING)
		{
			periphHomed = (motorStatus[0].homeState || !jacking_home_need[0]) && (motorStatus[1].homeState || !jacking_home_need[1]);
		}

		if (!periphHomed)
			ROS_INFO_THROTTLE(1, "peripheral motor homing check: jacking1[%d] jacking2[%d]", motorStatus[0].homeState, motorStatus[1].homeState);

		return periphHomed;
	}

	void AgvPeripheral::periphRecoverDetect()
	{
		bool recover = false;
		if (peripheral_type == JACKING)
			recover = jacking_recover_emit[0] || jacking_recover_emit[1];

		if (recover)
		{
			RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] recover detected.");
			if (PeripheralRecoverCtl.call(trig_srv))
			{
				RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] call recover service success");
			}
			else
			{
				RCLCPP_ERROR(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] call recover service fail");
			}
		}
		else if ((motorStatus[0].motorErr || motorStatus[1].motorErr) && (rclcpp::Time::now() - err_begin_time).toSec() > 10.0)
		{
			RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] long time motor error detected");
			if (PeripheralRecoverCtl.call(trig_srv))
			{
				RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] call recover service success");
			}
			else
			{
				RCLCPP_ERROR(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] call recover service fail");
			}

			err_begin_time = rclcpp::Time::now();
		}

		jacking_recover_emit[0] = false;
		jacking_recover_emit[1] = false;
	}

	void AgvPeripheral::periphStatusGather()
	{
		rclcpp::Time time_now = rclcpp::Time::now();
		periphStatus.header.stamp = time_now;

		if (abs(joint_pos[0] - min_pos[0]) > 0.005)
			periphStatus.loaded = true;
		else
			periphStatus.loaded = false;

		loads.clear();
		periphStatus.loads.clear();

		//使用loadtype字段
		loadInfo.loadPosition = periphStatus.loaded ? "1" : "0";
		loadInfo.loadType = loadInfo.loadPosition;
		if ((time_now - last_landmark_shelf_time).toSec() < 1.0)
		{
		}
		else
		{
			loadInfo.loadId = "";

			if (load_with_qrcode && periphStatus.loaded)
			{
				ROS_INFO_THROTTLE(3, "[AgvPeripheral] Package QRCode Lost !!!  now: %f  last: %f", time_now.toSec(), last_landmark_shelf_time.toSec());
				// periphEvent.nodeId = AGV_PERIPHERAL_CONTROLLER_NODE_ID;
				// periphEvent.nodeName = "agv_peripheral_controller_node";
				// periphEvent.eventLists.clear();

				// eventOccur.occur_id = 0;
				// eventOccur.event_details = "Package QRCode Lost !!!";
				// eventOccur.control_mode = agv_msgs::msg::EventOccur::EventStop;
				// eventOccur.occur = true;

				// std::vector<agv_msgs::msg::EventOccur> occurs;
				// occurs.push_back(eventOccur);
				// periphEvent.eventLists = occurs;

				// event_pub.publish(periphEvent);
				// last_event_time = time_now;
			}
		}

		loads.push_back(loadInfo);
		periphStatus.loads = loads;

		periphStatus.periphStateInfo = get_cur_state();
		periphStatus.periphMotor = periphMotor;
		periph_status_pub.publish(periphStatus);
	}

	bool AgvPeripheral::periphGetControllerRunningStatus()
	{
		return true;
	}

	bool AgvPeripheral::periphMoveToRecordPos()
	{

		if (!read_record_result)
		{
			RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "cannot read record. move to record cancled");
			return true;
		}

		// ROS_INFO_THROTTLE(0.2, "read file jacking position: %f  current jacking position: %f", file_record_pos[0], joint_pos[0]);
		// RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "read file jacking position: %f  current jacking position: %f", file_record_pos[0], joint_pos[0]);

		if (file_record_pos[0] > max_pos[0] || file_record_pos[0] < min_pos[0])
		{
			RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] bad record, quit. record_pos[%.4f] max_pos[%.4f] min_pos[%.4f]", file_record_pos[0], max_pos[0], min_pos[0]);
			return true;
		}

		if (abs(file_record_pos[0]) <= 0.0001)
		{
			RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] record home. finished");
			return true;
		}

		if (abs(joint_pos[0] - file_record_pos[0]) <= 0.0001 && abs(joint_pos[1] - file_record_pos[0]) <= 0.0001)
		{
			RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] reach record pos. finished");
			jacking_arrive_recordPos = true;
			jacking_move_state = false;
		}

		/*
		if(abs(joint_pos[1] - file_record_pos[1]) <= 0.0001)
		{
			rotary_arrive_recordPos = true;
			rotary_move_state = false;
		}
		*/

		if ((!jacking_move_state) && ((abs(joint_pos[0] - file_record_pos[0]) > 0.0001) || (abs(joint_pos[1] - file_record_pos[0]) > 0.0001)))
		{
			RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] jacking move to record pos, publish: %f current_pos: %f", file_record_pos[0], joint_pos[0]);
			setTargetJackingPosition(file_record_pos[0]);
			jacking_move_state = true;
			jacking_move_time = rclcpp::Time::now();
		}

		/*
		if ((!rotary_move_state) && (abs(joint_pos[1] - file_record_pos[1]) > 0.0001))
		{
			pallet_controller->set_rotary_target_position(file_record_pos[1]);
			rotary_move_state = true;
			rotary_move_time = rclcpp::Time::now();
			RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] rotary move to record pos, set target: %f", file_record_pos[1]);

		}
		*/

		if (jacking_move_state && ((rclcpp::Time::now() - jacking_move_time).toSec() > OPERATE_TIMEOUT_S))
		{
			if ((motorStatus[0].movingState && abs(joint_pos[0] - file_record_pos[0]) > 0.0001)
				|| (motorStatus[1].movingState && abs(joint_pos[1] - file_record_pos[0]) > 0.0001))
			{
				setTargetJackingPosition(joint_pos[0]);
				RCLCPP_ERROR(rclcpp::get_logger("AgvPeripheralController"), "jacking move to record pos timeout");
				jacking_move_state = false;
			}
		}

		/*
		if (rotary_move_state && ((rclcpp::Time::now() - rotary_move_time).toSec() > OPERATE_TIMEOUT_S))
		{
			if ((motorStatus[1].movingState) && (abs(joint_pos[1] - file_record_pos[1]) > 0.0001))
			{
				pallet_controller->set_rotary_target_position(joint_pos[1]);
				RCLCPP_ERROR(rclcpp::get_logger("AgvPeripheralController"), "jacking move to record pos timeout");
				rotary_move_state = false;
			}
		}
		*/

		return jacking_arrive_recordPos; //  & rotary_arrive_recordPos;
	}

	void AgvPeripheral::periphStateMachineUpdate()
	{
		if (!periphInitialized)
		{
			if (periphHomingCheck() && periphMotorEnableCheck() && periphGetControllerRunningStatus() && periphReadyCheck())
			{
				if (periphMoveToRecordPos())
				{
					periphInitialized = true;
					set_cur_state(IDLE_STATE);
				}
			}
			else
			{
				RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] drivers status check fail. call driver recover");
				set_cur_state(UNCONTROLLED_STATE);

				/*
				if (PeripheralRecoverCtl.call(trig_srv)) {
					RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] call recover service success");
				} else {
					RCLCPP_ERROR(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] call recover service fail");
				}

				rclcpp::Duration(1).sleep();
				*/
			}
		}
		if (periphInitialized)
		{
			switch (get_cur_state())
			{
			case IDLE_STATE:
				if (input_status.manualState)
				{
					manual_target_pos[0] = joint_pos[0];
					set_cur_state(MANUAL_MODE_STATE);
				}
				break;
			case MANUAL_MODE_STATE:
				ep_.clear(1,1);
				// if (input_status.emgButtonState)
				if ((read_input_data.emergency_stop_1 == 1 && last_read_input_data.emergency_stop_1 == 0) ||
					(read_input_data.emergency_stop_2 == 1 && last_read_input_data.emergency_stop_2 == 0))
				{
					if (PeripheralHaltCtl.call(trig_srv))
					{
						set_cur_state(HALT_HANDLE_STATE);
					}
					break;
				}
				if (!input_status.manualState)
				{
					set_cur_state(IDLE_STATE);
				}
				else
				{
					rclcpp::Time now = rclcpp::Time::now();
					if ((now - last_jacking_time).toSec() > jacking_timeout_time && manual_jacking_active)
					{
						RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] teleop jacking timeout. now[%.4f] last[%.4f]", now.toSec(), last_jacking_time.toSec());
						setTargetJackingPosition(joint_pos[0]);
						manual_jacking_active = false;
					}
				}
				break;
			case HALT_HANDLE_STATE:
				// if (!input_status.emgButtonState && !input_status.handle_emergency_stop && !input_status.front_and_rear_bumper)
				if (read_input_data.reset == 1 && last_read_input_data.reset == 0 &&
					read_input_data.emergency_stop_1 == 0 && read_input_data.emergency_stop_2 == 0)
				{
					// if (PeripheralRecoverCtl.call(trig_srv))
					// {
					RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] call recover success");
					set_cur_state(IDLE_STATE);
					// }
					// else
					// {
					// 	RCLCPP_ERROR(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] call recover fail");
					// }
				}
				break;
			case JACKING_OP_STATE_UP:
			case JACKING_OP_STATE_DOWN:
			case JACKING_SUCCESS_STATE:
				// if (input_status.emgButtonState || input_status.handle_emergency_stop || input_status.front_and_rear_bumper)
				if ((read_input_data.emergency_stop_1 == 1 && last_read_input_data.emergency_stop_1 == 0) ||
					(read_input_data.emergency_stop_2 == 1 && last_read_input_data.emergency_stop_2 == 0))
				{
					set_cur_state(HALT_HANDLE_STATE);
					if (PeripheralHaltCtl.call(trig_srv))
					{
						result.success = FAILED;
						result.message = "Operate halt";
						as.setSucceeded(result);
						set_cur_op(NO_OP);
						set_next_op(NO_OP);
						RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] call halt success,operation is Completed");
					}
					else
					{
						RCLCPP_ERROR(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] call recover fail");
					}
				}
				break;
			case UNCONTROLLED_STATE:
				if (get_cur_op())
				{
					result.success = FAILED;
					result.message = "Operate fail";
					as.setSucceeded(result);
					set_cur_op(NO_OP);
					set_next_op(NO_OP);
					RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] operation is uncompleted, motor is uncontrollable");
				}

				if (periphHomingCheck() && periphMotorEnableCheck() && periphGetControllerRunningStatus() && periphReadyCheck() && !periphMotorErrCheck())
				{
					set_cur_state(IDLE_STATE);
					RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] uncontrolled state recovered, set idle state");
				}

				break;
			default:
				break;
			}

			if (periphHomingCheck() && periphMotorEnableCheck() && periphGetControllerRunningStatus() && periphReadyCheck() && !periphMotorErrCheck())
			{
			}
			else
			{

				if (get_cur_state() != HALT_HANDLE_STATE)
				{
					set_cur_state(UNCONTROLLED_STATE);
					ROS_INFO_THROTTLE(1.0, "[AgvPeripheral] motor check error, set uncontrolled state");
				}
			}

			last_read_input_data.reset = read_input_data.reset;
			last_read_input_data.emergency_stop_1 = read_input_data.emergency_stop_1;
			last_read_input_data.emergency_stop_2 = read_input_data.emergency_stop_2;
		}
	}

	void AgvPeripheral::spin()
	{
		double period = 1.0 / frequency;
		rclcpp::Rate spin_rate(frequency);

		while (!shutdown_req && rclcpp::ok())
		{
			if (get_cur_op())
			{
				switch (get_cur_op())
				{
				case UP:
					if (abs(joint_pos[0] - max_pos[0]) < 0.0005 && abs(joint_pos[1] - max_pos[0]) < 0.0005)
					{
						set_cur_state(JACKING_SUCCESS_STATE);
						if (get_next_op())
						{
							feedback.state = get_cur_state();
							feedback.percent_complete = 1.0f;
							feedback.feedback_action = current_action;
							as.publishFeedback(feedback);
							set_cur_op((periph_op_enum)get_next_op());
							set_next_op(NO_OP);
							RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] Jacking UP Complete and Next Operate:%d", os.next_op);
							executeOP(get_cur_op());
						}
						else
						{
							result.success = SUCCESS;
							result.message = "Operate successed";
							result.result_action = current_action;
							as.setSucceeded(result);
							// memset(&current_action, 0, sizeof(current_action));
							RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] Jacking UP Complete");
							clear_os();
						}
					}
					else
					{
						feedback.state = get_cur_state();
						feedback.percent_complete = (abs(joint_pos[0] - previous_pos[0]) * 1.0f / abs(max_pos[0] - previous_pos[0])
							+ abs(joint_pos[1] - previous_pos[1]) * 1.0f / abs(max_pos[0] - previous_pos[1])) / 2;
						feedback.feedback_action = current_action;
						RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] Jacking Up Complete:%.2f,curent 1 pos is %.4f  2 pos is %.4f", feedback.percent_complete, joint_pos[0], joint_pos[1]);
						as.publishFeedback(feedback);
					}
					break;

				case DOWN:
					if (abs(joint_pos[0] - min_pos[0]) < 0.0005 && abs(joint_pos[1] - min_pos[0]) < 0.0005)
					{
						set_cur_state(JACKING_SUCCESS_STATE);
						result.success = SUCCESS;
						result.message = "Operate successed";
						result.result_action = current_action;
						as.setSucceeded(result);
						// memset(&current_action, 0, sizeof(current_action));
						RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] Jacking Down Complete");
						clear_os();
					}
					else
					{
						feedback.state = get_cur_state();
						feedback.percent_complete = (abs(joint_pos[0] - previous_pos[0]) * 1.0f / abs(min_pos[0] - previous_pos[0])
							+ abs(joint_pos[1] - previous_pos[1]) * 1.0f / abs(min_pos[0] - previous_pos[1])) / 2;
						feedback.feedback_action = current_action;
						RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] Jacking Down Complete:%.2f,curent 1 pos is %.4f  2 pos is %.4f", feedback.percent_complete, joint_pos[0], joint_pos[1]);
						as.publishFeedback(feedback);
					}
					break;

				default:
					break;
				}
			}

			if ((get_cur_op() == UP) || (get_cur_op() == DOWN))
			{
				if ((rclcpp::Time::now() - operate_time).toSec() > OPERATE_TIMEOUT_S)
				{
					RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] now time is %f,operate_time is %f", rclcpp::Time::now().toSec(), operate_time.toSec());
					if (((motorStatus[0].movingState) && (abs(joint_pos[0] - previous_pos[0]) < 0.0001))
						|| (motorStatus[1].movingState) && (abs(joint_pos[1] - previous_pos[1]) < 0.0001))
					{
						RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] movingState is %d %d,joint_pos is %.4f %.4f,previous_pos is %.4f %.4f", 
							motorStatus[0].movingState, motorStatus[1].movingState, joint_pos[0], joint_pos[1], previous_pos[0], previous_pos[1]);
						setTargetJackingPosition(joint_pos[0]);
						clear_os();
						result.success = FAILED;
						result.message = "Operate timeout";
						result.result_action = current_action;
						as.setSucceeded(result);
						// memset(&current_action, 0, sizeof(current_action));
						RCLCPP_ERROR(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] Jacking operate timeout");
					}
					else if ((rclcpp::Time::now() - operate_time).toSec() > MOVE_TIMEOUT_S)
					{
						clear_os();
						result.success = FAILED;
						result.message = "move timeout";
						result.result_action = current_action;
						as.setSucceeded(result);
						// memset(&current_action, 0, sizeof(current_action));
						RCLCPP_ERROR(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] Jacking move timeout");
					}
				}
			}
			
			periphStateMachineUpdate();

			periphStatusGather();

			set_stop_bits();

			spin_rate.sleep();
		}
	}
    void AgvPeripheral::setTargetJackingPosition(double pos)
    {
   		std_msgs::msg::Float64 target;
   		target.data = pos;
   		jacking_1_pub.publish(target);
   		jacking_2_pub.publish(target);
    }

	bool AgvPeripheral::executeOP(int order)
	{
		bool success = true;
		switch (order)
		{
		case UP:
			previous_pos[0] = joint_pos[0];
			previous_pos[1] = joint_pos[1];
			setTargetJackingPosition(max_pos[0]);
			RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] set order up, target pos is %.4f", max_pos[0]);
			set_cur_state(JACKING_OP_STATE_UP);
			break;

		case DOWN:
			previous_pos[0] = joint_pos[0];
			previous_pos[1] = joint_pos[1];
			setTargetJackingPosition(min_pos[0]);
			RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] set order down, target pos is %.4f", min_pos[0]);
			set_cur_state(JACKING_OP_STATE_DOWN);
			break;

		default:
			success = false;
			break;
		}

		if (!success)
		{
			result.success = FAILED;
			result.message = "Operation not defined";
			result.result_action = current_action;
			as.setSucceeded(result);
			// memset(&current_action, 0, sizeof(current_action));
			set_cur_state(IDLE_STATE);
			RCLCPP_WARN(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] Operate failed");
			return false;
		}
		return true;
	}

	void AgvPeripheral::goalCB()
	{
		action_pick = "pick";
		action_drop = "drop";
		action_adjust = "lhdAdjust";
		ros::param::get(PICK_NAME_CONFIG, action_pick);
		ros::param::get(DROP_NAME_CONFIG, action_drop);
		ros::param::get(ADJUST_NAME_CONFIG, action_adjust);

		if (as.isNewGoalAvailable())
		{
			auto goal_ptr = as.acceptNewGoal();
			if (goal_ptr.get())
			{
				RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "goalCB get pointer");
				current_action = goal_ptr->goal_action;
			}
			else
			{
				RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "goalCB get null pointer");

				result.success = FAILED;
				result.message = "Operate fail: bad pointer";
				as.setSucceeded(result);
				return;
			}
		}
		else
		{
			RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "goalCB  no goal available!");
			result.success = FAILED;
			result.message = "Operate fail: bad pointer";
			as.setSucceeded(result);
			return;
		}

		ROS_INFO_STREAM("goal action: " << current_action);

		double landmark_update_time = (rclcpp::Time::now() - last_landmark_time).toSec();
		RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "landmark_update_time: %.4f", landmark_update_time);

		if (nav_type == 1 && landmark_update_time > 0.5) {
			RCLCPP_ERROR(rclcpp::get_logger("AgvPeripheralController"), "goalCB  no landmark detected !!!");

			agv_msgs::msg::AGVEvent event;
			event.ecode = agv_msgs::msg::AGVEvent::LOAD_AND_UNLOAD_ERROR;
			event.type = agv_msgs::msg::AGVEvent::ALARM;
			event.action = agv_msgs::msg::AGVEvent::ESTOP;
			event.ackReq = true;
			event.param1 = 1;
			event.param2 = 0;
			event.description = "[Peripheral] no landmark detected !!!";
			ep_.add(event, 1, event.param1);

			result.success = FAILED;
			result.message = "Operate fail: vehicle not at landmark";
			as.setSucceeded(result);
			return;	
		}
		else if (nav_type == 1 && landmark_update_time <= 0.5) {
			ep_.clear(1, 1);
		}


		int action_type = 0;
		// int action_id = atoi(current_action.actionId.c_str());
		if (current_action.actionType.compare(action_pick) == 0 || current_action.actionType.compare(ORDER_UP_OLD_VERSION) == 0)
		{
			action_type = UP;
		}
		else if (current_action.actionType.compare(action_drop) == 0 || current_action.actionType.compare(ORDER_DOWN_OLD_VERSION) == 0)
		{
			action_type = DOWN;
		}
		else {
			action_type = NO_OP;
		}

		if (get_cur_state() != IDLE_STATE)
		{
			result.success = FAILED;
			result.message = "ERROR Device is busy";
			result.result_action = current_action;
			as.setSucceeded(result);
			// memset(&current_action, 0, sizeof(current_action));
			RCLCPP_WARN(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] last task is not completed");
			return;
		}

		// if (action_type > (int)DOWN && followup_flag) {
		// 	result.success = FAILED;
		// 	result.message = "ERROR Device followup";
		// 	result.result_action = current_action;
		// 	as.setSucceeded(result);
		// 	// memset(&current_action, 0, sizeof(current_action));
		// 	RCLCPP_WARN(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] last task is not completed");
		// 	return;

		// }

		switch (action_type)
		{

		case NO_OP:
		case UP:
		case DOWN:
			set_cur_op((periph_op_enum)action_type);
			set_next_op(NO_OP);
			break;

		default:
			break;
		}

		executeOP(action_type);
	}

	void AgvPeripheral::preemptCB()
	{
		RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] %s: Preempted", name.c_str());
		result.result_action = current_action;

		if (as.isActive())
		{
			if ((get_cur_op() == UP) || (get_cur_op() == DOWN))
			{
				setTargetJackingPosition(joint_pos[0]);
				RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "[AgvPeripheral] cancel the jacking joint goal");
			}
		}
		// set the action state to preempted
		result.success = CANCELED;
		result.message = "Action has been preempted";
		result.result_action = current_action;
		as.setPreempted(result);
		// memset(&current_action, 0, sizeof(current_action));
	}

	bool AgvPeripheral::periphRecordPosRead()
	{
		std::fstream fin(pos_record_file.c_str(), std::ios::in);
		if (fin.fail())
		{
			RCLCPP_ERROR(rclcpp::get_logger("AgvPeripheralController"), "could not open %s.", pos_record_file.c_str());
			return false;
		}

		try
		{
			YAML::Node yamlConfig = YAML::Load(fin);
			fin.close();
			if (!yamlConfig)
			{
				RCLCPP_ERROR(rclcpp::get_logger("AgvPeripheralController"), "read %s faild. data is not yaml format", pos_record_file.c_str());
				return false;
			}
			if (yamlConfig["jacking_joint_pos"] && yamlConfig["rotary_joint_pos"])
			{
				file_record_pos[0] = yamlConfig["jacking_joint_pos"].as<float>();
				file_record_pos[1] = yamlConfig["rotary_joint_pos"].as<float>();
				if (file_record_pos[0] > max_pos[0])
					file_record_pos[0] = max_pos[0];
				if (file_record_pos[0] < 0.00)
					file_record_pos[0] = 0.00;
				if (file_record_pos[1] > M_PI)
					file_record_pos[1] = M_PI;
				if (file_record_pos[1] < -1 * M_PI)
					file_record_pos[1] = -1 * M_PI;
				RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "read pos success, jacking joint pos is %.4f rotary joint pos is %.4f", file_record_pos[0], file_record_pos[1]);
			}
			else
			{
				RCLCPP_ERROR(rclcpp::get_logger("AgvPeripheralController"), "read %s faild. data is not exist", pos_record_file.c_str());
				return false;
			}
		}
		catch (const YAML::Exception &e)
		{
			RCLCPP_ERROR(rclcpp::get_logger("AgvPeripheralController"), "file format error %s,%s.", pos_record_file.c_str(), e.what());
			return false;
		}
		return true;
	}

	bool AgvPeripheral::periphRecordPosWrite()
	{
		// TODO: shouldn't open/close every seconds.

		std::fstream fout(pos_record_file.c_str(), std::ios::out);
		if (fout.fail())
		{
			RCLCPP_ERROR(rclcpp::get_logger("AgvPeripheralController"), "could not creat file %s.", pos_record_file.c_str());
			return false;
		}

		YAML::Node yamlConfig;
		yamlConfig["jacking_joint_pos"] = joint_pos[0];
		yamlConfig["rotary_joint_pos"] = joint_pos[1];
		// RCLCPP_INFO(rclcpp::get_logger("AgvPeripheralController"), "write jacking joint pos %.4f rotary joint pos %.4f", joint_pos[0], joint_pos[1]);
		fout << yamlConfig;
		fout.flush();
		fout.close();

		return true;
	}

	void AgvPeripheral::periphPosRecord()
	{
		if (IDLE_STATE == get_cur_state() || MANUAL_MODE_STATE == get_cur_state())
		{
			if (!periphRecordPosWrite())
			{
				RCLCPP_ERROR(rclcpp::get_logger("AgvPeripheralController"), "write record pos fail");
			}
		}
	}

	void AgvPeripheral::thread_cycle()
	{
		periphPosRecord();
		check_jointState_out();
	}

	void AgvPeripheral::thread_fun(AgvPeripheral *pThis)
	{
		if (!pThis)
			return;

		while (!pThis->m_quit_mutex.try_lock_for(std::chrono::milliseconds(1000)))
		{
			pThis->thread_cycle();
		}
	}

}; // end namespace

/** @brief main function */
int main(int argc, char **argv)
{
	// Initialize the ROS node
	rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared("agv_peripheral_controller");

	std::shared_ptr<agv_peripheral::AgvPeripheral> peripheral;
	std::shared_ptr<boost::thread> worker_thread;
	peripheral.reset(new agv_peripheral::AgvPeripheral("peripheral"));
	peripheral->init();

	worker_thread.reset(new boost::thread(&agv_peripheral::AgvPeripheral::spin, peripheral));

	rclcpp::Rate r(100);
	while (rclcpp::ok())
	{
		rclcpp::spin_some(node);
		r.sleep();
	}

	peripheral->shutdown();
	worker_thread->join();

	return 0;
}
